#include <iostream>

#include <ros/ros.h>
#include "gpd_ros/GraspConfigList.h"
#include "gpd_ros/GraspConfig.h"
#include "tf/transform_listener.h"
#include "tf/transform_broadcaster.h"
#include "tf_conversions/tf_eigen.h"
#include "eigen_conversions/eigen_msg.h"
#include "std_msgs/Float64MultiArray.h" 
#include "std_srvs/Empty.h"

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>

class BaseNav{
 public:
  explicit BaseNav(){

    gpd_hand_sub_ = nh_.subscribe<gpd_ros::GraspConfigList>("/detect_grasps/clustered_grasps",1,&BaseNav::GraspHandle,this);


    nav_conf_pub_ = nh_.advertise<std_msgs::Float64MultiArray>("nav_conf",1);

    target_pose_pub_ = nh_.advertise<geometry_msgs::TransformStamped>(
          "target_pose", 1);
    
    // nav_pose_pub_ = nh_.advertise<geometry_msgs::TransformStamped>("nav_pose",1);

    nav_enable_srvs_ = nh_.advertiseService(
        "base_nav/nav_enable", &BaseNav::NavEnableCallback, this);

    conf_nav_unlock_srvs_ = nh_.advertiseService(
        "base_nav/unlock", &BaseNav::UnlockCallback, this);

    nav_ongoing_flag_ = false;
    // nav_action_flag_ = false;
    nav_action_flag_ = true;    
  }

  void GraspHandle(const gpd_ros::GraspConfigList::ConstPtr &msg){
    //没有就返回
    if(msg->grasps.size() < 1){
      ROS_INFO("Didn't have any grasp in msg");
      return;
    }

    //监听坐标系
    tf::StampedTransform transform;
    try {
      tf_listener_.lookupTransform("/base_link", "/zedm_left_camera_frame", 
                                   ros::Time(0), transform);
    }
    catch (tf::TransformException &ex) {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
      return;
    }

    double distance_ = 999;//初始给个最大值
    std::vector<double> nav_point_ {0, 0, 0};
    std::vector<double> target_pose_ {0, 0, 0, 0, 0, 0};
    Eigen::Vector3d target_point_(0, 0, 0);
    Eigen::Matrix3d grasp_rotation;
    grasp_rotation.Identity();//初始化成单位矩阵

    for(int i=0;i<msg->grasps.size();i++){
    //先接受转换
    Eigen::Quaterniond eigen_quat;  // should be Eigen::Quaterniond
    tf::quaternionTFToEigen(transform.getRotation(), eigen_quat);
    Eigen::Vector3d eye_trans;
    tf::vectorTFToEigen(transform.getOrigin(), eye_trans);

    //目标坐标的转换
    Eigen::Vector3d target_position_eye(msg->grasps[i].position.x,
                                 msg->grasps[i].position.y,
                                 msg->grasps[i].position.z);
    Eigen::Vector3d grasp_position_base = eigen_quat.matrix() * target_position_eye + 
                                    eye_trans;  // trans target into base

    //approach的转换
    Eigen::Vector3d approach_eye(msg->grasps[i].approach.x,
                                msg->grasps[i].approach.y,
                                msg->grasps[i].approach.z);
    Eigen::Vector3d approach_base = eigen_quat.matrix() * approach_eye;//这是得到接近的方向
    approach_base.normalize();

    // std::cout << "approach_base" << approach_base << std::endl;

    //求导航点相对于baselink的坐标值
    double d1 = 0.291, d2 = 0.4, d3 = 0.41, base_joint_x = -0.1345;
    //d1=0.305,joint-x=-0.1345
    Eigen::Vector3d OB = grasp_position_base - d3 * approach_base;
    Eigen::Vector3d OZ_op_(0,0,-1);
    Eigen::Vector3d Bias_to_base_(0,0,-d1);
    Eigen::Vector3d B_XY = -OB.dot(OZ_op_) * OZ_op_;
    // std::cout << "B_XY" << B_XY << std::endl;

    Eigen::Vector3d A_XY = -grasp_position_base.dot(OZ_op_) * OZ_op_;
    // std::cout << "A_XY" << A_XY << std::endl;

    Eigen::Vector3d OC = OB + B_XY - Bias_to_base_;

    // std::cout << "OC" << OC << std::endl;
    // std::cout << "panduan" << (B_XY - Bias_to_base_).norm() << std::endl;

    if((B_XY - Bias_to_base_).norm() > d2){
      continue;//这就是没解的情况
    }
    // std::cout << "OC2" << OC << std::endl;

    double EC = std::sqrt(d2 * d2 - (B_XY - Bias_to_base_).norm() * (B_XY - Bias_to_base_).norm());
    Eigen::Vector3d OD = grasp_position_base + A_XY - Bias_to_base_;
    Eigen::Vector3d DC = OC - OD;
    DC(2)=0;//确保z轴是0
    DC.normalize();//目前车头朝向的方向向量
    Eigen::Vector3d CE = EC * DC;
    Eigen::Vector3d OE = OC + CE;
    Eigen::Vector3d EF = base_joint_x * DC;//此处DC表示车的朝向
    Eigen::Vector3d OF = OE + EF;
    Eigen::Vector3d OH = OF + Bias_to_base_;//这是导航点
    Eigen::Vector3d OX(1,0,0);
    Eigen::Vector3d HA = grasp_position_base - OH;

    // std::cout << "OH" << OH << std::endl;
    
    //求距离目前最近的导航点
    double distance_cal_ = OH.norm();
    if(distance_cal_ < distance_){
      distance_ = distance_cal_;
      double Nav_angle = std::atan2(-DC(1),-DC(0));//in rad
      //导航点
      nav_point_[0] = OH(0);
      nav_point_[1] = OH(1);
      nav_point_[2] = Nav_angle;

      //binormal的转换
      Eigen::Vector3d grasp_direction_eye(msg->grasps[i].binormal.x,
                                 msg->grasps[i].binormal.y,
                                 msg->grasps[i].binormal.z);
      Eigen::Vector3d grasp_direction_base = eigen_quat.matrix() * grasp_direction_eye;
      grasp_direction_base.normalize();
      //axis的转换
      Eigen::Vector3d axis_eye(msg->grasps[i].axis.x,
                                msg->grasps[i].axis.y,
                                msg->grasps[i].axis.z);
      Eigen::Vector3d axis_base = eigen_quat.matrix() * axis_eye;//这是得到接近的方向
      axis_base.normalize();

      //目标点在老坐标系的值
      target_point_ = grasp_position_base;

      //抓取坐标系在base_link里的三个向量
      grasp_rotation << approach_base, grasp_direction_base, axis_base;
    }
    }

    if(target_point_.norm() ==0){
      return ;
    }


    //广播导航点的坐标系
    static tf::TransformBroadcaster br;
    ros::Time publish_time = ros::Time::now();
    geometry_msgs::TransformStamped Nav_pose;
    Nav_pose.header.frame_id = "base_link";
    Nav_pose.header.stamp = publish_time;
    Nav_pose.child_frame_id = "NAV_link";
    Nav_pose.transform.translation.x = nav_point_[0];
    Nav_pose.transform.translation.y = nav_point_[1];
    Nav_pose.transform.translation.z = 0;//这个置0，防止没必要的精度问题
    Eigen::Vector3d Euler(nav_point_[2], 0, 0);
    Eigen::Quaterniond quaternion3;
    quaternion3 = Eigen::AngleAxisd(Euler[0], Eigen::Vector3d::UnitZ()) * 
                  Eigen::AngleAxisd(Euler[1], Eigen::Vector3d::UnitY()) * 
                  Eigen::AngleAxisd(Euler[2], Eigen::Vector3d::UnitX());
    quaternion3.normalize();
    tf::quaternionEigenToMsg(quaternion3,Nav_pose.transform.rotation);
    // nav_pose_pub_.publish(Nav_pose);//这个用不用还没想好
    br.sendTransform(Nav_pose);//用于可视化，和之后逆解坐标系的发布
     
    //打印导航点
    // printf("Nav point[%.8lf,%.8lf,%.8lf]",nav_point_[0],nav_point_[1],nav_point_[2]);
    std::cout<< "Nav point["  << nav_point_[0] <<", "
                              << nav_point_[1] <<", "
                              << nav_point_[2] <<"]"<< std::endl;
    std::cout<< "grasp point["<< target_point_[0] <<", "
                              << target_point_[1] <<", "
                              << target_point_[2] <<"]"<< std::endl;


    //广播抓取点的坐标系
    geometry_msgs::TransformStamped target_pose;
    target_pose.header.frame_id = "base_link";
    target_pose.header.stamp = publish_time;
    target_pose.child_frame_id = "target_link";
    tf::vectorEigenToMsg(target_point_,target_pose.transform.translation);
    Eigen::Quaterniond quaternion4(grasp_rotation);
    quaternion4.normalize();
    tf::quaternionEigenToMsg(quaternion4,target_pose.transform.rotation);
    br.sendTransform(target_pose);//为了逆解坐标系发布

    if(nav_action_flag_ && !nav_ongoing_flag_){
      geometry_msgs::TransformStamped inverse_pub_;
      tf::StampedTransform transform_pub_;
      //监听两者关系
      try {
        tf_listener_.waitForTransform("/NAV_link", "/target_link",
                                   publish_time, ros::Duration(3.0));
        tf_listener_.lookupTransform("/NAV_link", "/target_link", 
                                   publish_time, transform_pub_);
      }
      catch (tf::TransformException &ex) {
        ROS_ERROR("%s",ex.what());
        ros::Duration(1.0).sleep();
       return;
      }

      //发布导航点和目标点
      tf::transformStampedTFToMsg(transform_pub_,inverse_pub_);
      std_msgs::Float64MultiArray Nav_point_;
      Nav_point_.data.resize(3);
      Nav_point_.data = nav_point_;

      target_pose_pub_.publish(inverse_pub_);
      nav_conf_pub_.publish(Nav_point_);

      nav_stamp_ = ros::Time::now();
      // nav_ongoing_flag_ = true;
      // nav_action_flag_=false;
    }

    if ((ros::Time::now() - nav_stamp_).toSec() > 25 &&
      nav_ongoing_flag_) {//如果超时25秒，并且还在抓取状态，就整个超时，其实没有必要了，这玩意肯定会反馈回来，除非那边程序挂了，这个可能就是提高鲁棒性
      nav_ongoing_flag_ = false;
      ROS_ERROR("nav overtime!!!");
    }  // overtime wathcer

  }


 private:
  ros::NodeHandle nh_;
  ros::Subscriber gpd_hand_sub_;
  ros::Publisher nav_conf_pub_;
  ros::Publisher target_pose_pub_;
  ros::Publisher nav_pose_pub_;
  ros::Time nav_stamp_;
  tf::TransformListener tf_listener_;
  ros::ServiceServer nav_enable_srvs_;
  ros::ServiceServer conf_nav_unlock_srvs_;
  // static tf::TransformBroadcaster br;
  bool nav_action_flag_;//之后平板创建服务用吧
  bool nav_ongoing_flag_;

  bool NavEnableCallback(std_srvs::EmptyRequest &request,
                          std_srvs::EmptyResponse &response) {
    nav_action_flag_ = true;//创建这个服务，就是为了吧这个action整成true
    ROS_INFO("Nav enable!");
    return true;
  }
  bool UnlockCallback(std_srvs::EmptyRequest &request,
                      std_srvs::EmptyResponse &response) {
    nav_ongoing_flag_ = false;
    ROS_INFO("nav unlock!");
    return true;
  }


};



int main(int argc, char **argv){
    ros::init(argc, argv, "base_nav");
    BaseNav Nav;
    ros::spin();
    return 0;
}